import random
import json


with open(f'src/simulator/simulator/quadrotor_simulator/config/multi_posi.json', 'r') as f:
    drone_data = json.load(f)

drone_num = len(drone_data)
ixs = [v['position'][0] for v in drone_data.values()]
iys = [v['position'][1] for v in drone_data.values()]
iyaws = [round(random.uniform(-3.14, 3.14), 2) for _ in range(drone_num)]

# with open('src/car_swarm_planner/traj_planner/config/launch_head_1.11','r') as f:
#     launch_head=[]
#     launch_head=f.read()
with open('src/simulator/simulator/quadrotor_simulator/config/launch_1.11','r') as f:
    launch_lines=[]
    for line in f.readlines():
         launch_lines.append(line)


with open(f'src/simulator/simulator/quadrotor_simulator/config/swarm_quadrotor_block.launch','w') as f:
  # f.write(launch_head)
  row_in_all = 0
  id_in_all = 0
  for drone_id in range(drone_num):      
    for line in launch_lines:
        if '"drone_id"' in line:
            f.write(f'      <arg name="drone_id" value="{drone_id}"/>\n')
        elif '"init_x"' in line:
            f.write(f'      <arg name="init_x" value="{ixs[drone_id]}"/>\n')
        elif '"init_y"' in line:
            f.write(f'      <arg name="init_y" value="{iys[drone_id]}"/>\n')
        elif '"init_yaw"' in line:
            f.write(f'      <arg name="init_yaw" value="{iyaws[drone_id]}"/>\n')
        else:
            f.write(f'{line}')
    f.write("\n\n")

  # f.write('</launch>\n')
  # f.write('<!--the launch file is generated by multi-vehicle generator.py  -->')
  print ("all down!")
